/* 3pi_app2 - an application for the Pololu 3pi Robot
 *
 * This application uses the Pololu AVR C/C++ Library.  For help, see:
 * -User's guide: http://www.pololu.com/docs/0J20
 * -Command reference: http://www.pololu.com/docs/0J18
 *
 * Created: 3/5/2013 5:19:09 PM
 *  Author: Jimmeh
 */

#include <pololu/3pi.h>

const unsigned int SENSOR_THRESHOLD = 900;

const unsigned short SENSOR_LEFT = 1;
const unsigned short SENSOR_MID = 4;
const unsigned short SENSOR_RIGHT = 2;

// These are initialized to 0 by default, so they will not affect the track state
// Can be expanded by setting them to 8 and 16 and adding more track states
const unsigned short STABILIZER_LEFT = 0; // 8
const unsigned short STABILIZER_RIGHT = 0; // 16

unsigned short sensorStates[5];

// The combined states of the left, right and mid sensors will give us
// one of the following track states
const unsigned short OFF_TRACK = 0;
const unsigned short CORNER_LEFT = 1; // SENSOR_LEFT
const unsigned short CORNER_RIGHT = 2; // SENSOR_RIGHT
const unsigned short WALL = 3; // SENSOR_LEFT + SENSOR_RIGHT
const unsigned short ON_TRACK = 4; // SENSOR_MID
const unsigned short ON_WALL_LEFT = 5; // SENSOR_MID + SENSOR_LEFT
const unsigned short ON_WALL_RIGHT = 6; // SENSOR_MID + SENSOR_RIGHT
const unsigned short INTERSECTION = 7; // SENSOR_MID + SENSOR_LEFT + SENSOR_RIGHT


#define STOP 0
#define FORWARD 1
#define REVERSE 2
#define TURN_LEFT 3
#define TURN_RIGHT 4
#define SPIN_LEFT 5
#define SPIN_RIGHT 6

void init_tracker() {
	sensorStates[0] = SENSOR_LEFT;
	sensorStates[1] = STABILIZER_LEFT;
	sensorStates[2] = SENSOR_MID;
	sensorStates[3] = STABILIZER_RIGHT;
	sensorStates[4] = SENSOR_RIGHT;
}

unsigned short cur_track_state = 0;
unsigned short new_track_state = 0;
long track_state_time = 0;
unsigned short get_track_state(const unsigned int* sensors);

int main()
{
	
	unsigned int sensors[5];
	
	init_tracker();
	
	pololu_3pi_init(1000);
	
	play_from_program_space(PSTR(">g32>>c32"));  // Play welcoming notes.

	//motors_init();
	
	//set_m1_speed(50);
	//set_m2_speed(50);
	
	int speed = 70;
	int followLine = 0;
	int adjustment = 0;
	
	unsigned short trackState = OFF_TRACK;
	unsigned short curTrackState;
	
	long delayTime = 0;
	
	unsigned short motorState = STOP;
	unsigned short turnSwitch = TURN_LEFT;
	
	while(1)
	{		
		clear();
		
		//print_long(get_ms());
		
		if(get_single_debounced_button_press(BUTTON_B)) {
			if(motorState != STOP)
				motorState = STOP;
			else
				motorState = FORWARD;
		}
		
		read_line_sensors(sensors, IR_EMITTERS_ON);
		curTrackState = get_track_state(sensors);
		
		switch(motorState) {
			case FORWARD:
				if(curTrackState != trackState) {
					
					trackState = curTrackState;
					
					if(trackState == INTERSECTION) {
						play_from_program_space(PSTR(">g32>>c32"));
						motorState = turnSwitch;
						turnSwitch = (turnSwitch == TURN_LEFT ? TURN_RIGHT : TURN_LEFT);
					}
					else if(trackState == CORNER_LEFT) {
						motorState = TURN_LEFT;
					}							
					else if(trackState == CORNER_RIGHT) {
						motorState = TURN_RIGHT;
					}
					else if(trackState == WALL) {
						motorState = TURN_LEFT + (rand() % 2);
					}
					else if(trackState == ON_WALL_RIGHT && (rand() % 2) == 1) {
						motorState = TURN_RIGHT;
					}
					else if(trackState == ON_WALL_LEFT && (rand() % 2) == 1) {
						motorState = TURN_LEFT;
					}
					delayTime = get_ms() + 200;
				}
				adjustment = ((int)sensors[3] - (int)sensors[1])/20;
				set_motors(speed+adjustment, speed-adjustment);
				break;
			case TURN_LEFT:
				set_motors(-speed/2, speed);
				if(get_ms() > delayTime && curTrackState == ON_TRACK) {
					trackState = ON_TRACK;
					motorState = FORWARD;
				}
				break;
			case TURN_RIGHT:
				set_motors(speed, -speed/2);
				if(get_ms() > delayTime && curTrackState == ON_TRACK) {
					trackState = ON_TRACK;
					motorState = FORWARD;
				}
				break;
			case STOP:
				set_motors(0, 0);
				break;
		}
	}		
}

unsigned short get_track_state(const unsigned int* sensors) {
	unsigned short trackState = OFF_TRACK;
	
	for(size_t i = 0; i < 5; i++) {
		if(sensors[i] > SENSOR_THRESHOLD) {
			trackState += sensorStates[i];
		}
	}
	
	if(trackState != new_track_state) {
		new_track_state = trackState;
		track_state_time = get_ms() + 22;
	}
	
	if(new_track_state != cur_track_state && get_ms() > track_state_time) {
		cur_track_state = new_track_state;
	}
	
	return cur_track_state;
}
